﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Timers;
using System.Windows.Forms;
using Trio.Motor;
using Atc600Control;

namespace RollController2022
{
    public partial class Form2 : Form
    {
        public Form2()
        {
            InitializeComponent();
        }
        System.Timers.Timer getvalue_timer = new System.Timers.Timer();//位置速度取值定时器

        int move_axis = 10;//轴10为左电机；
        int back_axis = 3; //轴3为右电机；
        int Encoder_axis = 0; //编码器轴0；
        int Tension_axis = 9; //左磁粉DA9；
        int tape_axis = 11;//胶带轴11
        TrioP600 trio = new TrioP600();//轴10为左电机；轴3为右电机；编码器轴0；左磁粉DA9；
        Atc600 atc600 = new Atc600();
        private void Form2_Load(object sender, EventArgs e)
        {
            SetMode();//模式选择显示隐藏输入框
            trio.Connect();//通讯连接
            trio.RollInitMpos(Encoder_axis);//编码器清零
            atc600.Connect();//通讯连接
            trio.SetTension(Tension_axis, 1);//左磁粉初始
            atc600.SetTension(0, 1);//右磁粉手动值1N
            getvalue_timer.Interval = 50;
            getvalue_timer.Elapsed += new ElapsedEventHandler(value_Elapsed);
        }

        private void Form2_FormClosed(object sender, FormClosedEventArgs e)
        {
            timer1.Stop();//停止显示
            getvalue_timer.Stop();//停止读取
            trio.Closed();//停止运动，断开连接
            atc600.Closed();//断开连接
        }

        private void btn_Clear_Click(object sender, EventArgs e)
        {
            numUpD_LSpeed.Enabled = true;
            numUpD_LTorque.Enabled = true;
            cb_LReverse.Enabled = true;

            numUpD_Overrate.Enabled = true;
            numUpD_Hts.Enabled = true;
            numUpD_Tape.Enabled = true;
            btn_Calculate.Enabled = true;

            numUpD_RSpeed.Enabled = true;
            numUpD_RTorque.Enabled = true;
            cb_RReverse.Enabled = true;

            btn_Face.Enabled = true;
            btn_Back.Enabled = true;

            rbtn_Wrap.Enabled = true;
            rbtn_Back.Enabled = true;
            cb_Length.Enabled = true;
            tbx_Length.Enabled = true;

            btn_L.Enabled = true;
            btn_R.Enabled = true;
            btn_Clear.Enabled = true;

            lb_Speed.Text = "0.00";
            lb_Length.Text = "0.00";

            btn_Start.Text = "Start";
        }

        double n;//转/秒
        private void btn_Start_Click(object sender, EventArgs e)
        {
            if (btn_Start.Text == "Start")
            {
                numUpD_LSpeed.Enabled = false;
                numUpD_LTorque.Enabled = false;
                cb_LReverse.Enabled = false;

                numUpD_Overrate.Enabled = false;
                numUpD_Hts.Enabled = false;
                numUpD_Tape.Enabled = false;
                btn_Calculate.Enabled = false;

                numUpD_RSpeed.Enabled = false;
                numUpD_RTorque.Enabled = false;
                cb_RReverse.Enabled = false;

                btn_Face.Enabled = false;
                btn_Back.Enabled = false;

                rbtn_Wrap.Enabled = false;
                rbtn_Back.Enabled = false;
                cb_Length.Enabled = false;
                tbx_Length.Enabled = false;

                btn_L.Enabled = false;
                btn_R.Enabled = false;
                btn_Clear.Enabled = false;

                trio.RollInitMpos(Encoder_axis);

                SetMove();
                lasterror = error = piterm = diterm = 0;//PID清零
                getvalue_timer.Start();//打开定时器
                timer1.Start();

                btn_Start.Text = "Stop";
            }
            else if (btn_Start.Text == "Continue")
            {
                btn_Clear.Enabled = false;
                cb_Length.Enabled = false;
                tbx_Length.Enabled = false;

                SetMove();
                lasterror = error = piterm = diterm = 0;//PID清零
                getvalue_timer.Start();//打开定时器
                timer1.Start();

                btn_Start.Text = "Stop";
            }
            else if (btn_Start.Text == "Stop")
            {
                btn_Clear.Enabled = true;
                cb_Length.Enabled = true;
                tbx_Length.Enabled = true;

                timer1.Stop();//停止显示
                getvalue_timer.Stop();//停止读取、PID
                trio.StopMove();//停止运动
                trio.SetTension(Tension_axis, 1);//左磁粉初始
                atc600.SetTension(0, 1);//右磁粉手动值1N

                btn_Start.Text = "Continue";
            }
        }

        private void numUpD_Correct_ValueChanged(object sender, EventArgs e)
        {
            trio.CtrlSpeed(tape_axis, 8000 * Math.Round((double)numUpD_Correct.Value, 2));
        }

        double set_speed, set_backspeed;
        double length_read, speed_read, angle_read;
        DateTime dt_last;
        TimeSpan Time;
        double Encoder, Encoder_last;
        private void value_Elapsed(object sender, ElapsedEventArgs e)
        {
            //读取速度，长度,角度
            angle_read = 1;

            Encoder_last = Encoder;//储存编码器
            Encoder = Math.Abs(trio.GetEncoder(Encoder_axis));//编码器值，轴0

            length_read = Encoder / 8000.0 * 0.16 * 3.1415926 / 1000;//编码器换算总长度m
            angle_read = Encoder % 8000 / 360;//计算角度

            DateTime dt_now = DateTime.Now;
            Time = dt_now.Subtract(dt_last);

            speed_read = Math.Abs(Encoder_last - Encoder) / 8000.0 * 0.16 * 3.1415926 / 1000 / Time.TotalMilliseconds * 1000;//计算速度m/s
            dt_last = dt_now;

            if (rbtn_Wrap.Checked == true)//圆，恒线
            {
                trio.CtrlSpeed(move_axis, PidSpeed(set_speed, speed_read));//左侧轴10
                CheckLimit();
            }
            else if (rbtn_Back.Checked == true)
            {
                trio.CtrlSpeed(back_axis, PidSpeed(set_backspeed, speed_read));//右侧轴3
            }
        }

        private void CheckLimit()
        {
            if (cb_Length.Checked == true)//长度限制
            {
                if (Convert.ToDouble(length_read) > Convert.ToDouble(tbx_Length.Text))
                {
                    MessageBox.Show("");
                }
            }
        }

        DateTime PID_lasttime;
        TimeSpan PID_changetime;
        float Kp = 1, Kd = 18;
        float lasterror, error, piterm, diterm;
        private double PidSpeed(double set, double read)
        {
            DateTime dt_now = DateTime.Now;
            PID_changetime = dt_now.Subtract(PID_lasttime);
            PID_lasttime = dt_now;

            double output_V;

            error = (float)(set - read);
            piterm = Kp * error;
            diterm = (float)(Kd * (error - lasterror) * 1000 / PID_changetime.TotalMilliseconds);

            output_V = set / 0.16 / 3.1415926 * 8000 * 0.9 + (piterm + diterm);

            if (output_V < 0)
            {
                output_V = 0;
            }
            if (output_V > 2000)
            {
                output_V = 2000;
            }
            lasterror = error;
            return output_V;
        }
        private void timer1_Tick(object sender, EventArgs e)
        {
            lb_Speed.Text = Math.Round(speed_read, 2).ToString();
            lb_Length.Text = Math.Round(length_read, 2).ToString();
        }

        //胶带轴手动
        private void btn_Face_MouseDown(object sender, MouseEventArgs e)
        {
            trio.RoollInitMotor(tape_axis, 0);//设置轴与方向
            trio.CtrlSpeed(tape_axis, 100);
        }
        private void btn_Face_MouseUp(object sender, MouseEventArgs e)
        {
            trio.CtrlSpeed(tape_axis, 0);
        }
        private void btn_Back_MouseDown(object sender, MouseEventArgs e)
        {
            trio.RoollInitMotor(tape_axis, 1);//设置轴与方向
            trio.CtrlSpeed(tape_axis, 100);
        }
        private void btn_Back_MouseUp(object sender, MouseEventArgs e)
        {
            trio.CtrlSpeed(tape_axis, 0);
        }

        //手动运动
        private void btn_L_MouseDown(object sender, MouseEventArgs e)
        {
            int L_dir = 0;
            if (cb_LReverse.Checked == true) L_dir = 1;
            else if (cb_LReverse.Checked == false) L_dir = 0;
            trio.RoollInitMotor(move_axis, L_dir);//设置轴与方向
            trio.SetTension(Tension_axis, 50);//左张力10V
            atc600.SetTension(1, 1);//右张力设置1N
            trio.CtrlSpeed(move_axis, 500);
        }
        private void btn_L_MouseUp(object sender, MouseEventArgs e)
        {
            trio.CtrlSpeed(move_axis, 0);
            trio.StopMove();
            trio.SetTension(Tension_axis, 1);//左磁粉初始
            atc600.SetTension(0, 1);//右磁粉手动值1N
        }
        private void btn_R_MouseDown(object sender, MouseEventArgs e)
        {
            int R_dir = 0;
            if (cb_RReverse.Checked == true) R_dir = 1;
            else if (cb_RReverse.Checked == false) R_dir = 0;
            trio.RoollInitMotor(back_axis, R_dir);//设置轴与方向
            trio.SetTension(Tension_axis, 1);//左张力1V
            atc600.SetTension(0, 50);//右张力设置10N
            trio.CtrlSpeed(back_axis, 500);
        }
        private void btn_R_MouseUp(object sender, MouseEventArgs e)
        {
            trio.CtrlSpeed(back_axis, 0);
            trio.StopMove();
            trio.SetTension(Tension_axis, 1);//左磁粉初始
            atc600.SetTension(0, 1);//右磁粉手动值1N
        }

        double overrate, hts, tape;
        private void btn_Calculate_Click(object sender, EventArgs e)
        {
            MessageBox.Show(Calculate().ToString());
        }
        private double Calculate()
        {
            double angle = 0;
            overrate = Math.Round((double)(numUpD_Overrate.Value), 0) * 0.01;
            hts = Math.Round((double)(numUpD_Hts.Value), 0);
            tape = Math.Round((double)(numUpD_Tape.Value), 0);

            angle = Math.Round(Math.Acos(tape * (1 / overrate) / (2 * hts)), 0);
            return angle;
        }

        private void rbtn_Wrap_CheckedChanged(object sender, EventArgs e)
        {
            SetMode();
        }
        private void rbtn_Back_CheckedChanged(object sender, EventArgs e)
        {
            SetMode();
        }

        private void SetMove()
        {
            set_speed = Math.Round((double)numUpD_LSpeed.Value, 2);//m/s
            set_backspeed = Math.Round((double)numUpD_RSpeed.Value, 2);

            int L_dir = 0, R_dir = 0, M_dir = 0;
            if (cb_LReverse.Checked == true) L_dir = 1;
            else if (cb_LReverse.Checked == false) L_dir = 0;

            if (cb_RReverse.Checked == true) R_dir = 1;
            else if (cb_RReverse.Checked == false) R_dir = 0;

            if (cb_MReverse.Checked == true) M_dir = 1;
            else if (cb_MReverse.Checked == false) M_dir = 0;

            if (rbtn_Wrap.Checked == true)//恒线
            {
                trio.RoollInitMotor(move_axis, L_dir);//走线轴10与方向
                trio.RoollInitMotor(tape_axis, M_dir);//包绕轴11与方向
                trio.SetTension(Tension_axis, 50);//左张力10V
                atc600.SetTension(1, (int)numUpD_RTorque.Value);//右张力设置

                n = set_speed * Math.Asin(Calculate()) / (Math.Round((double)(numUpD_Hts.Value), 0) *
                    (1 - 0.01 * Math.Round((double)(numUpD_Overrate.Value), 0))) / 60;//转/s
                n = Math.Round(n, 2);
                trio.CtrlSpeed(tape_axis, 8000 * n);
                numUpD_Correct.Value = (decimal)n;
            }
            else if (rbtn_Back.Checked == true)//退回
            {
                trio.RoollInitMotor(back_axis, R_dir);//设置轴与方向
                trio.SetTension(Tension_axis, (double)numUpD_LTorque.Value);//左张力设置
                atc600.SetTension(0, 50);//右张力10V，非恒张力
            }
        }

        private void SetMode()
        {
            if (rbtn_Wrap.Checked == true)//包绕，恒线
            {
                numUpD_LTorque.Visible = false;//左张力
                lb_LTorque.Visible = false;
                numUpD_LSpeed.Visible = true;//左线速度
                lb_LSpeed.Visible = true;
                numUpD_RTorque.Visible = true;//右张力
                lb_RTorque.Visible = true;
                numUpD_RSpeed.Visible = false;//右线速度
                lb_RSpeed.Visible = false;
            }
            else if (rbtn_Back.Checked == true)//退回
            {
                numUpD_LTorque.Visible = true;//左张力
                lb_LTorque.Visible = true;
                numUpD_LSpeed.Visible = false;//左线速度
                lb_LSpeed.Visible = false;
                numUpD_RTorque.Visible = false;//右张力
                lb_RTorque.Visible = false;
                numUpD_RSpeed.Visible = true;//右线速度
                lb_RSpeed.Visible = true;
            }
        }
    }
}
